#include "command.h"
#include "stdio.h"
#include "string.h"
#include "usart.h"


/* 用户定义的函数头文件 */

#include "main.h"
#include "delay.h"
#include "foc.h"					// 调试电流环
#include "speed.h"					// 调试速度环
#include "position.h"				// 调试位置环

#include "motor_a_foc_config.h"			// 调试电流环
#include "motor_a_speed_config.h"		// 调试速度环
#include "motor_a_position_config.h"	// 调试位置环

extern FOC_Struct MotorA_FOC;							// 调试电流环
extern SPEED_Control_Struct MotorA_SPEED_Control;		// 调试速度环
extern POSITION_Control_Struct MotorA_POSITION_Control;	// 调试位置环

/* ******************** */

uint8_t rx_buffer[100];					// 接收数据的数组
volatile uint8_t rx_len		= 0; 		// 接收数据的长度
volatile uint8_t needPro	= 0;		// 需要处理的标志


/* 命令回调函数 */
typedef void (*cmd_cb)(char*);


/* 命令结构体 */
typedef struct
{
	char* cmd;	// 接收到的命令
	cmd_cb cb;	// 对应命令的回调函数
}ST_CMD;


// 先把下面的函数先声明下
void AT_help_proc(char* str);
void AT_led_proc(char* str);
void AT_pid_proc(char* str);


/* 用户需要把定义的命令和对应的函数添加进来 */
/* 命令列表 */
ST_CMD cmd_list[] = 
{
	{"?",	AT_help_proc},
	{"led",	AT_led_proc},
	{"pid",	AT_pid_proc},
};


/**
 * @brief   "?"命令的回调函数，打印所有的命令。
 *
 * @param   none
 *
 * @return  none
 */
void AT_help_proc(char* str)
{
	int i, size = sizeof(cmd_list)/sizeof(ST_CMD);
	
	printf("this is helper~\r\n");
	for(i = 0; i < size; i++)
	{
		printf("%s \r\n", cmd_list[i].cmd);
	}
}

/**
 * @brief   "led"命令的回调函数，打印所有的命令。eg：[led -h]、[led off]、[led on]、[led blink 5]
 *
 * @param   none
 *
 * @return  none
 */
void AT_led_proc(char* str)
{
	printf("led\r\n");
	if(strstr(str, "-h"))
	{
		printf("you can use [on/off] ctrl led \r\n");
		return;
	}
	if(strstr(str, "on"))
	{
		HAL_GPIO_WritePin(EN_A_GPIO_Port, EN_A_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(EN_B_GPIO_Port, EN_B_Pin, GPIO_PIN_RESET);
		return;
	}
	if(strstr(str, "off"))
	{
		HAL_GPIO_WritePin(EN_A_GPIO_Port, EN_A_Pin, GPIO_PIN_SET);
		HAL_GPIO_WritePin(EN_B_GPIO_Port, EN_B_Pin, GPIO_PIN_SET);
		return;
	}
	if(strstr(str, "blink"))
	{
		int count, ret;
		ret = sscanf(str, "blink %d", &count);
		if(ret == 1)
		{
			printf("led begin blink %d time \r\n", count);
			for(int i = 0; i < count*2; i++)
			{
				HAL_GPIO_TogglePin(EN_A_GPIO_Port, EN_A_Pin);
				HAL_GPIO_TogglePin(EN_B_GPIO_Port, EN_B_Pin);
				delay_ms(100);
			}
		}
	}
}

/**
 * @brief   "pid"命令的回调函数，打印所有的命令。eg：[pid p=0,i=0,d=0,tq=0,td=0]
 *
 * @param   none
 *
 * @return  none
 */
void AT_pid_proc(char* str)
{
	int ret;

	// 电流环调试
	float kp, ki, kd, tarq, tard;
	// ret = sscanf(str, "p=%f,i=%f,d=%f,tq=%f,td=%f", &kp, &ki, &kd, &tarq, &tard);
	ret = sscanf(str, "p=%f,i=%f,d=%f", &kp, &ki, &kd);
	if(ret == 3)
	{
		SetCurrentPIDParams(&MotorA_FOC, kp, ki, kd, 12.0f * 0.577f);
		// MOTORA_SetTarIDIQ(tard, tarq);
	}
	
	// 速度环调试
//	float kp, ki, kd, tars;
//	ret = sscanf(str, "p=%f,i=%f,d=%f,ts=%f", &kp, &ki, &kd, &tars);
//	if(ret == 4)
//	{
//		SetSpeedPIDParams(&MotorA_SPEED_Control, kp, ki, kd);
//		MOTORA_SPEED_Config_SetTar(tars);
//	}
	
	// 位置环调试
//	float kp, ki, kd, tarp;
//	ret = sscanf(str, "p=%f,i=%f,d=%f,tp=%f", &kp, &ki, &kd, &tarp);
//	if(ret == 4)
//	{
//		SetPositionPIDParams(&MotorA_POSITION_Control, kp, ki, kd);
//		MOTORA_POSITION_Config_SetTar(tarp);
//	}
}

/* ================================================= */

/**
 * @brief   寻找字符串中非空格数据的位置
 *
 * @param   none
 *
 * @return  none
 */
char *skip(char *buf)
{
	while(*buf == ' ')
	{
		buf++;
	}
	return buf;
}

/**
 * @brief   处理函数，对比接收到的命令是否在命令列表中，并执行。
 *
 * @param   buf		接收到的命令
 *
 * @return  none
 */
void USART_DATA_Proc(char* buf)
{
	if(needPro == 0)
	{
		return;
	}
	
	int i, size = sizeof(cmd_list)/sizeof(ST_CMD);
	
	for(i = 0; i < size; i++)
	{
		// cmd列表中命令的长度
		int len = strlen(cmd_list[i].cmd);
		
		if(strncmp(buf, cmd_list[i].cmd, len) == 0)
		{
			char *s = skip(buf + len);
			// 传入的参数是：接收到的命令去掉了前面的cmd命令后
			// 第一个不是空格的字符开始，一直到结束
			cmd_list[i].cb(s);
		}
	}
	
	needPro = 0;	// 处理完成之后清除标志
}

/**
 * @brief   串口DMA初始化
 *
 * @param   buf		接收到的命令
 *
 * @return  none
 */
void USART_DATA_Init(void)
{
	// 开启空闲串口中断
	__HAL_UART_ENABLE_IT(&huart1, UART_IT_IDLE);
	// 开始接收数据
	HAL_UART_Receive_DMA(&huart1, (uint8_t *)rx_buffer, 100);
}
